#ifndef DYNAMICS_SENSORS_H
#define DYNAMICS_SENSORS_H

struct ContactSensor
{
  int link;                ///<the link on which the sensor is located
  RigidTransform Tsensor;  ///<local frame of the sensor (by convention, z is normal to surface, into robot)
  bool contact;            ///<true if contact has been made
  Vector3 force;           ///<the force vector
  bool activeDirs[3];      ///<if an element is true, that component of force is measured
};

struct JointForceSensor
{
  int link;                ///<the link on which the sensor is located (between link and parent)
  Vector3 localPos;        ///<the position of the sensor, in the local frame
  Vector3 f,m;             ///<the force/moment at the given position, on the link (negative on the parent link)
  bool hasForce;           ///<true if force is measured
  bool hasMoment;          ///<true if moment is measured
};

struct Accelerometer
{
  int link;
  Vector3 pos;             ///< position of unit on link
  Vector3 accel;           ///< acceleration value
  Vector3 accumVel;        ///< integration of accel
  Vector3 accumPos;        ///< double integration of accel
};

struct GyroSensor
{
  int link;
  Vector3 angvel;
  Matrix3 rotation;
};

struct GravitySensor 
{
  int link;               ///< the link on which the sensor is located
  Vector3 gravity;        ///< the gravity vector local to the link
  Matrix3 gravityVariance;///< the variance associated with the measurement
};

struct RobotSensors
{
  Config q,dq;  //joint position variables
  Vector qvariance,dqvariance;
  Vector qresolution,dqresolution;
  bool hasJointPosition,hasJointVelocity;

  vector<ContactSensor> contactSensors;
  vector<JointForceSensor> jointForceSensors;
  vector<Accelerometer> accelerometers;
  vector<GyroSensor> gyroSensors;
  vector<GravitySensor> gravitySensors;
};

#endif
